#include <linux/kernel.h>
#include <linux/sysfs.h>
#include <linux/stat.h>
#include <linux/slab.h>
#include <linux/device.h>
#include <linux/gpio.h>

#include "movement.h"
#include "pwm.h"

////////////////////////////////////////// Motor ///////////////////////////////////

static ssize_t motor_value_show(struct device *dev, struct device_attribute *attr, char *buf);
static ssize_t motor_value_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count);

static void motor_release_dev(struct device *dev)
{
	struct motor_device *motor_device = container_of(dev, struct motor_device, device);

	gpio_set_value(motor_device->enable_pin, 0);
	gpio_free(motor_device->enable_pin);

	gpio_pwm_release(motor_device->pwm_pin);
	gpio_pwm_release(motor_device->rev_pin);

	device_remove_file(dev->parent,&motor_device->value_attr);

	kfree(dev->init_name);
	kfree(dev);
}

struct motor_device* motor_init(unsigned int pwm_pin, unsigned int rev_pin, unsigned int enable_pin, char *name, struct device *parent)
{
	struct motor_device *dev = kmalloc(sizeof(struct motor_device), GFP_KERNEL);
	size_t len = strlen(name) + 1;

	if(!dev)
		return (struct motor_device*)-ENOMEM;

	memset(dev, 0, sizeof(struct motor_device));                   // init data to zero

	dev->device.init_name = kmalloc(len, GFP_KERNEL);              // init data
	memset(dev->device.init_name, 0, len);
	strncpy(dev->device.init_name, name, len);
	dev->device.parent = parent;
	dev->device.release = &motor_release_dev;

	dev->value_attr.attr.name = "value";
	dev->value_attr.attr.mode = S_IRUGO | S_IWUGO;                 // read/write
	dev->value_attr.show = &motor_value_show;                      // read handler
	dev->value_attr.store = &motor_value_store;                    // write handler

	if(!gpio_request(enable_pin,"GPIO_1")) {                       // disable board first
		gpio_direction_output(enable_pin,0);
		dev->enable_pin = enable_pin;
	}
	else
		return (struct motor_device*)-ENODEV;

	dev->pwm_pin = gpio_pwm_init(pwm_pin);                        // allocate gpio_pin to be used by pwm as in pwm.c
	gpio_pwm_set_polarity(dev->pwm_pin, 1);
	gpio_pwm_set_period(dev->pwm_pin, MOTOR_PWM_PERIOD);

	if((int)dev->pwm_pin == -ENODEV)
		return (struct motor_device*)-ENODEV;

	dev->rev_pin = gpio_pwm_init(rev_pin);                        // allocate gpio_pin to be used by pwm as in pwm.c
	gpio_pwm_set_polarity(dev->rev_pin, 1);
	gpio_pwm_set_period(dev->rev_pin, MOTOR_PWM_PERIOD);

	if((int)dev->rev_pin == -ENODEV)
		return (struct motor_device*)-ENODEV;

	int ret = device_register(&dev->device);
	if(ret) return (struct motor_device*)-ENODEV;

	ret = device_create_file(&dev->device,&dev->value_attr);
	if(ret)
		return (struct motor_device*)ret;

	return dev;
}

void motor_release(struct motor_device *dev)
{
	device_unregister(&dev->device);
}

/////////////////// Motor pwm ////////////////////////

static ssize_t motor_value_show(struct device *dev, struct device_attribute *attr, char *buf)
{
	struct motor_device *motor_device = container_of(dev, struct motor_device, device);
	return sprintf(buf, "%d\n", motor_device->value);
}

static ssize_t motor_value_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
{
	struct motor_device *motor_device = container_of(dev, struct motor_device, device);

	int pwm;
	sscanf(buf, "%d", &pwm);

	if(pwm > 255)
		pwm = 255;
	else if(pwm < -255)
		pwm = -255;

	if(pwm > 0) {
		// set direction pin set enable pin pwm new speed
		gpio_pwm_set_duty(motor_device->pwm_pin, pwm * MOTOR_PWM_PERIOD / 255);
		gpio_pwm_stop(motor_device->rev_pin);
		gpio_pwm_start(motor_device->pwm_pin);
		gpio_set_value(motor_device->enable_pin, 1);
	}
	else if(pwm < 0) {
		// clear direction pin set enable pin pwm new speed
		gpio_pwm_set_duty(motor_device->rev_pin, -pwm * MOTOR_PWM_PERIOD / 255);
		gpio_pwm_stop(motor_device->pwm_pin);
		gpio_pwm_start(motor_device->rev_pin);
		gpio_set_value(motor_device->enable_pin, 1);
	}
	else {
		// disable motors and stop pwm
		gpio_set_value(motor_device->enable_pin, 0);
		gpio_pwm_stop(motor_device->pwm_pin);
		gpio_pwm_stop(motor_device->rev_pin);
	}

	motor_device->value = pwm;

	return count;
}

/////////////////// Motor Breaking Is Now Depricated For Safety ////////////////////

///////////////////////////////////// Servo ////////////////////////////////////////

static void servo_release_dev(struct device *dev)
{
	struct servo_device *servo_device = container_of(dev, struct servo_device, device);

	gpio_pwm_release(servo_device->desc);

	device_remove_file(dev->parent,&servo_device->attr);
	kfree(dev->init_name);
	kfree(dev);
}

static ssize_t servo_show(struct device *dev, struct device_attribute *attr, char *buf);
static ssize_t servo_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count);

struct servo_device* servo_init(unsigned int gpio_pin, char *name, struct device *parent)
{
	struct servo_device *dev = kmalloc(sizeof(struct servo_device), GFP_KERNEL);
	size_t len = strlen(name) + 1;

	if(!dev)
		return (struct servo_device*)-ENOMEM;

	memset(dev, 0, sizeof(struct servo_device));                 // init data to zero

	dev->device.init_name = kmalloc(len, GFP_KERNEL);          // init data
	memset(dev->device.init_name, 0, len);
	strncpy(dev->device.init_name, name, len);
	dev->device.parent = parent;
	dev->device.release = &servo_release_dev;

	dev->attr.attr.name = "value";
	dev->attr.attr.mode = S_IRUGO | S_IWUGO;                   // read/write
	dev->attr.show = &servo_show;                              // read handler
	dev->attr.store = &servo_store;                            // write handler

	dev->desc = gpio_pwm_init(gpio_pin);                       // allocate gpio_pin to be used by pwm as in pwm.c
	gpio_pwm_set_polarity(dev->desc, 1);

	if((int)dev->desc == -ENODEV)
		return (struct servo_device*)-ENODEV;

	int ret = device_register(&dev->device);
	if(ret) return (struct servo_device*)-ENODEV;

	ret = device_create_file(&dev->device,&dev->attr);
	if(ret)
		return (struct servo_device*)ret;

	return dev;
}

void servo_release(struct servo_device *dev)
{
	device_unregister(&dev->device);
}

/////////////////// Servo file ops //////////////////////

static ssize_t servo_show(struct device *dev, struct device_attribute *attr, char *buf)
{
	struct servo_device *servo_device = container_of(dev, struct servo_device, device);

	sprintf(buf, "%d\n", servo_device->value);
	return strlen(buf);
}

static ssize_t servo_store(struct device *dev, struct device_attribute *attr, const char *buf, size_t count)
{
	// 0 to 255 anything negative disables servo

	struct servo_device *servo_device = container_of(dev, struct servo_device, device);

	int value = 0;
	sscanf(buf, "%d", &value);
	if(value < 0) {                                     // disable servo
		servo_device->enabled = 0;
		gpio_pwm_stop(servo_device->desc);
	}
	else {                                             // set value; enable and start if disabled
		if(value > 255)
			value = 255;

		servo_device->value = value;
		gpio_pwm_set_duty(servo_device->desc, (value + 30) * (2000000 - 200000) / 255 + 200000);

		// min value for servo is .75 ms max 2.25 ms
		// oh wait no the data sheet provided by
		// parallax was wrong its actually much prettier
		// my mesurements show .2 ms to 2 ms

		gpio_pwm_set_period(servo_device->desc, 20000000);      // 20 ms
		if(!servo_device->enabled) {
			servo_device->enabled = 1;
			gpio_pwm_start(servo_device->desc);
		}
	}
	return count;
}
